﻿#include "TesQuaternion.h"
#include "TesVector3.h"
#include "TesMatrix3x3.h"
#include "TesEuler.h"
#include <assert.h>

#ifndef DEGREE_TO_RADIAN
#define DEGREE_TO_RADIAN(X) (X / 180.0f * PAI)
#endif

void TesQuaternion::Identity()
{
	w = 1.0f;
	x = y = z = 0.0f;
}

float TesQuaternion::Length()
{
	return sqrtf(x * x + y * y + z * z + w * w);
}

void TesQuaternion::Normalize()
{
	float Temp = sqrtf(x * x + y * y + z * z + w * w);
	if (Temp > 0.0f)
	{
		x *= (1 / Temp);
		y *= (1 / Temp);
		z *= (1 / Temp);
		w *= (1 / Temp);
	}
	else 
	{
		assert(false);
		Identity();
	}
}

void TesQuaternion::RotateX(float degree)
{
	float radian = -(degree / 180.0f * PAI);
	w = cosf(radian / 2.0f);
	x = sinf(radian / 2.0f);
	y = z = 0.0f;
}

void TesQuaternion::RotateY(float degree)
{
	float radian = -(degree / 180.0f * PAI);
	w = cosf(radian / 2);
	y = sinf(radian / 2);
	x = z = .0f;
}

void TesQuaternion::RotateZ(float degree)
{
	float radian = -(degree / 180.0f * PAI);
	w = cosf(radian / 2);
	z = sinf(radian / 2);
	x = y = .0f;
}

void TesQuaternion::RotateAxis(float degree, const TesVector3& Axis)
{
	float radian = -(degree / 180.0f * PAI);
	TesVector3 ax = Axis.Normalize();
	w = cosf(radian / 2);
	x = ax.x * sinf(radian / 2);
	y = ax.y * sinf(radian / 2);
	z = ax.z * sinf(radian / 2);
}

TesQuaternion TesQuaternion::operator * (const TesQuaternion& that)
{
	TesQuaternion Res;

	Res.w = w * that.w - x * that.x - y * that.y - z * that.z;
	Res.x = w * that.x + x * that.w + z * that.y - y * that.z;
	Res.y = w * that.y + y * that.w + x * that.z - z * that.x;
	Res.z = w * that.z + z * that.w + y * that.x - x * that.y;
	return Res;
}

TesQuaternion& TesQuaternion::operator *= (const TesQuaternion& that)
{
	*this = *this * that;
	return *this;
}

TesQuaternion TesQuaternion::operator * (float k)
{
	TesQuaternion Res;
	Res.w = this->w * k;
	Res.x = this->x * k;
	Res.y = this->y * k;
	Res.z = this->z * k;
	return Res;
}

TesQuaternion& TesQuaternion::operator *= (float k)
{
	this->w = this->w * k;
	this->x = this->x * k;
	this->y = this->y * k;
	this->z = this->z * k;
	return *this;
}

float TesQuaternion::DotProduct(const TesQuaternion& that)  const
{
	return w * that.w + x * that.x + y * that.y + z * that.z;
}

TesQuaternion TesQuaternion::Conjugate()
{
	TesQuaternion Res;
	Res.w = this->w;
	Res.x = -this->x;
	Res.y = -this->y;
	Res.z = -this->z;
	return Res;
}

TesQuaternion TesQuaternion::Inverse()
{
	TesQuaternion Res;
	Res = this->Conjugate();
	Res = Res * (1.0f / this->Length());
	return Res;
}

TesQuaternion TesQuaternion::operator -(const TesQuaternion& that)
{
	TesQuaternion Res;
	Res = this->Inverse() * that;
	return Res;
}

TesQuaternion TesQuaternion::Pow(float t)
{
	if (w > 0.99999f) {
		return *this;
	}
	TesQuaternion Res;
	float alpha = acosf(w);
	float newAlpha = alpha * t;
	float K = sinf(newAlpha) / sinf(alpha);
	Res.w = cosf(newAlpha);
	Res.x = x * K;
	Res.y = y * K;
	Res.z = z * K;
	return Res;
}

TesMatrix3x3 TesQuaternion::Matrix3x3()
{
	TesMatrix3x3 Res;
	Res.M11_3 = 1 - 2 * y * y - 2 * z * z;
	Res.M12_3 = 2 * x * y + 2 * w * z;
	Res.M13_3 = 2 * x * z - 2 * w * y;
	Res.M21_3 = 2 * x * y - 2 * w * z;
	Res.M22_3 = 1 - 2 * x * x - 2 * z * z;
	Res.M23_3 = 2 * y * z + 2 * w * x;
	Res.M31_3 = 2 * x * z + 2 * w * y;
	Res.M32_3 = 2 * y * z - 2 * w * x;
	Res.M33_3 = 1 - 2 * x * x - 2 * y * y;
	return Res;
}

TesVector3 TesQuaternion::operator* (const TesVector3& N)
{

	TesQuaternion temp;
	temp.w = 0; temp.x = N.x; temp.y = N.y; temp.z = N.z;
	temp = *this * temp * this->Inverse();
	return TesVector3(temp.x, temp.y, temp.z);
}

float TesQuaternion::GetRotationAngleRedius() {
  	return acosf(w) * 2;
}

float TesQuaternion::GetRotationAngleDegree() {
  	return GetRotationAngleRedius() / PAI * 180.f;
}

TesVector3 TesQuaternion::GetRotationAxis() {

	//w得实际值为cos(theta / 2)
	float sinThetaOver2Sq = 1.0f - w * w;

	//保证精度
	if (sinThetaOver2Sq < 0.0f)
		return TesVector3(1.0f, 0.0f, 0.0f);

	//计算 1 / sin(theta / 2)
	float oneOverSinThetaOver2 = 1.0f / sqrtf(sinThetaOver2Sq);

	//返回生成得向量
  	return TesVector3(x * oneOverSinThetaOver2, y * oneOverSinThetaOver2, z * oneOverSinThetaOver2);
}

void TesQuaternion::FromEuler(const TesEular& orientation)
{
	float sp, sb, sh;
	float cp, cb, ch;

	sp = sinf(DEGREE_TO_RADIAN(orientation.pitch) / 2.0f);
	sb = sinf(DEGREE_TO_RADIAN(orientation.bank) / 2.0f);
	sh = sinf(DEGREE_TO_RADIAN(orientation.heading) / 2.0f);

	cp = cosf(DEGREE_TO_RADIAN(orientation.pitch) / 2.0f);
	cb = cosf(DEGREE_TO_RADIAN(orientation.bank) / 2.0f);
	ch = cosf(DEGREE_TO_RADIAN(orientation.heading) / 2.0f);

	w = ch * cp * cb + sh * sp * sb;
	x = -ch * sp * cb - sh * cp * sb;
	y = ch * sp * sb - sh * cb * cp;
	z = sh * sp * cb - ch * cp * sb;
}

void TesQuaternion::ToEuler(TesEular& euler)
{
	//计算sin(pitch)
	float sp = -2.0f * (y * z - w * x);

	//检查万向锁
	if (fabs(sp) > 0.9999f)
	{
		euler.pitch = -(PAI  * 0.5f * sp);
		euler.heading = -(atan2f(-x * z + w * y, 0.5f - y * y - z * z));
		euler.bank = 0.0f;
	}
	else 
	{
		euler.pitch = -asinf(sp);
		euler.heading = -atan2f(x * z + w * y, 0.5f - x * x - y * y);
		euler.bank = -atan2f(x * y + w * z, 0.5f - x * x - z * z);
	}
}

TesQuaternion operator *(float k, TesQuaternion q)
{
	TesQuaternion Res;
	Res.w = q.w * k;
	Res.x = q.x * k;
	Res.y = q.y * k;
	Res.z = q.z * k;
	return Res;
}


TesQuaternion Conjugate(const TesQuaternion& q)
{
	TesQuaternion Res;
	Res.w = q.w;
	Res.x = -q.x;
	Res.y = -q.y;
	Res.z = -q.z;
	return Res;
}

float DotProduct(const TesQuaternion& a, const TesQuaternion& b)
{
	return a.DotProduct(b);
}

TesVector3 operator* (const TesVector3& N, TesQuaternion q)
{
	return q * N;
}

TesQuaternion Slerp(TesQuaternion& q0, TesQuaternion& q1, float t) 
{
	//边界检查
	if (t <= 0.0f) return q0;
	if (t >= 1.0f) return q1;

	//计算四元数得夹角
	float cosOmega = q0.DotProduct(q1);

	//如果点乘为负数，使用-q1，主要是是需要使用锐角插值
	float q1w = q1.w;
	float q1x = q1.x;
	float q1y = q1.y;
	float q1z = q1.z;

	if (cosOmega < 0.0f)
	{
		q1w = -q1.w;
		q1x = -q1.x;
		q1y = -q1.y;
		q1z = -q1.z;
		cosOmega = -cosOmega;
	}

	//这有要判断两个四元数是否单位化了
	assert(cosOmega < 1.01f);

	//插值系数
	float k0, k1;

	//非常接近的时候防止除0
	if (cosOmega > 0.99999f)
	{
		k0 = 1 - t;
		k1 = t;
	}
	else 
	{
		//由余弦得正弦
		float sinOmega = sqrtf(1.0f - cosOmega * cosOmega);

		//得到角度
		float omega = atan2f(sinOmega, cosOmega);

		//先计算结果后面只需要除一次
		float oneOverSinOmega = 1.0f / sinOmega;

		k0 = sinf((1.0f - t) * omega) * oneOverSinOmega;
		k1 = sinf(t * omega) * oneOverSinOmega;
	}

	//进行插值
	TesQuaternion result;
	result.x = k0 * q0.x + k1 * q1x;
	result.y = k0 * q0.y + k1 * q1y;
	result.z = k0 * q0.z + k1 * q1z;
	result.w = k0 * q0.w + k1 * q1w;
	return result;
}



